#include "imu.hpp"

#include <commons/util/crc.hpp>
#include <commons/util/util.hpp>
#include <commons/type/datetime.hpp>

#include <iomanip>

namespace sros::sensor
{
    static const uint8_t HEAD_FLAG    = 0x68; // 标志符
    static const uint8_t REQ_04_CMD   = 0x04; // 读取角度数据命令
    static const uint8_t RSP_84_CMD   = 0x84; // 反馈角度数据命令
    static const uint8_t REQ_0C_CMD   = 0x0C; // 设置传感器输出模式命令
    static const uint8_t RSP_8C_CMD   = 0x8C; // 设置传感器输出模式回复命令
    static const uint8_t REQ_0B_CMD   = 0x0B; // 设置波特率命令
    static const uint8_t RSP_8B_CMD   = 0x8B; // 设置波特率回复命令
    static const uint8_t REQ_28_CMD   = 0x28; // 方位角清零命令
    static const uint8_t RSP_28_CMD   = 0x28; // 方位角清零回复命令

    IMU::IMU(const hal::UartParam &uart_param, const Param &img_param)
        : impl::noncopyable()
        , uart_(nullptr)
        , param_(nullptr)
    {
        init_(uart_param, img_param);
    }

    void IMU::run()
    {
        uart_->run();
    }

    void IMU::close()
    {
        uart_->close();
    }

    RetBool IMU::set_output_mode(const uint8_t &mode)
    {
        std::vector<uint8_t> data{HEAD_FLAG, 0x05, param_->addr, 0x8C, mode, crc::crc8_bcc_sum({0x05, param_->addr, 0x8C, mode})};
        auto size = uart_->write_some(data);
        if (size != data.size())
        {
            std::string reason = util::format("real write byte size: {}, data size: {}", size, data.size());
            return {false, reason};
        }

        return {true};
    }

    RetBool IMU::set_bitrate(const uint8_t &bitrate)
    {
        std::vector<uint8_t> data{HEAD_FLAG, 0x05, param_->addr, 0x0B, bitrate, crc::crc8_bcc_sum({0x05, param_->addr, 0x0B, bitrate})};
        auto size = uart_->write_some(data);
        if (size != data.size())
        {
            std::string reason = util::format("real write byte size: {}, data size: {}", size, data.size());
            return {false, reason};
        }

        return {true};
    }

    RetBool IMU::req_data()
    {
        std::vector<uint8_t> data{HEAD_FLAG, 0x04, param_->addr, 0x04, crc::crc8_bcc_sum({0x04, param_->addr, 0x04})};
        auto size = uart_->write_some(data);
        if (size != data.size())
        {
            std::string reason = util::format("real write byte size: {}, data size: {}", size, data.size());
            return {false, reason};
        }

        return {true};
    }

    RetBool IMU::reset()
    {
        std::vector<uint8_t> data{HEAD_FLAG, 0x04, param_->addr, 0x28, crc::crc8_bcc_sum({0x04, param_->addr, 0x28})};
        auto size = uart_->write_some(data);
        if (size != data.size())
        {
            std::string reason = util::format("real write byte size: {}, data size: {}", size, data.size());
            return {false, reason};
        }

        return {true};
    }

    void IMU::init_(const hal::UartParam &uart_param, const Param &imu_param)
    {
        if (param_ || uart_)
        {
            return;
        }

        // 9轴数据反馈函数如果没有赋值，则赋值一个默认的
        param_ = std::make_shared<Param>(imu_param);
        if (!param_->on_asix_data_arrival)
        {
            param_->on_asix_data_arrival = [this](const uint64_t &&timestamp, const Vector9d<float> &&msg)
            {
                sros_debug << "[This default on_asix_data_arrival]: " << timestamp << sros_endl;
            };
        }

        // 当读取到串口数据，进行解析，并返回原始串口数据和解析后的数据
        auto uart_param_ = uart_param;
        auto on_msg_arrival = uart_param_.on_msg_arrival;
        auto on_error = uart_param_.on_error;
        uart_param_.on_msg_arrival = [this, on_msg_arrival, on_error](const hal::UartRecvMsg &&msg) -> void
        {
            std::vector<uint8_t> buff(msg.buff);
            std::size_t size = msg.size;
            hal::UartRecvMsg bk_msg {
                .buff = std::move(buff),
                .size = std::move(size)
            };

            if (on_msg_arrival)
            {
                on_msg_arrival(std::move(bk_msg));
            }

            // 校验基础数据大小
            if (msg.size < 5)
            {
                if (on_error)
                {
                    on_error(-1, "recv msg byte size is less than 5.");
                }
                return;
            }

            // 校验标识符
            if (msg.buff.at(0) != HEAD_FLAG)
            {
                if (on_error)
                {
                    on_error(-1, "recv msg first byte is not equal 0x68.");
                }
                return;
            }

            // 校验CRC校验码
            std::vector<uint8_t> cacl_buffer(msg.buff.begin() + 1, msg.buff.end() - 1);
            uint8_t calc_crc = crc::crc8_bcc_sum(cacl_buffer);
            uint8_t crc = msg.buff.at(msg.size-1);
            if (calc_crc != crc)
            {
                if (on_error)
                {
                    std::string reason = util::format("recv msg crc: {} calc_crc: {}", (int)crc, (int)calc_crc);
                    on_error(-1, std::move(reason));
                }
                return;
            }

            // 校验数据大小
            uint8_t data_length = msg.buff.at(1);
            if (msg.size - 1 != data_length)
            {
                if (on_error)
                {
                    std::string reason = util::format("recv msg data_length: {} real_data_length: {}", (int)data_length, msg.size - 1);
                    on_error(-1, std::move(reason));
                }
                return;
            }

            // 校验地址码
            uint8_t addr = msg.buff.at(2);
            if (addr != param_->addr)
            {
                if (on_error)
                {
                    std::string reason = util::format("recv msg addr: {} set addr: {}", (int)addr, (int)param_->addr);
                    sros_info << reason << sros_endl;
                    on_error(-1, std::move(reason));
                }
                return;
            }

            uint8_t cmd = msg.buff.at(3);
            switch (cmd)
            {
            case RSP_84_CMD:
            {
                std::vector<uint8_t> payload(msg.buff.begin() + 4, msg.buff.end() - 1);

                std::string roll_str;
                for (int i = 0; i < 3 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    roll_str += ss.str();
                }

                std::string pitch_str;
                for (int i = 3; i < 6 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    pitch_str += ss.str();
                }

                std::string yaw_str;
                for (int i = 6; i < 9 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    yaw_str += ss.str();
                }

                std::string acc_x_str;
                for (int i = 9; i < 12 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    acc_x_str += ss.str();
                }

                std::string acc_y_str;
                for (int i = 12; i < 15 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    acc_y_str += ss.str();
                }

                std::string acc_z_str;
                for (int i = 15; i < 18 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    acc_z_str += ss.str();
                }

                std::string gyro_x_str;
                for (int i = 18; i < 21 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    gyro_x_str += ss.str();
                }

                std::string gyro_y_str;
                for (int i = 21; i < 24 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    gyro_y_str += ss.str();
                }

                std::string gyro_z_str;
                for (int i = 24; i < 27 && i < payload.size(); i++)
                {
                    std::stringstream ss;
                    ss << std::hex << std::setw(2) << std::setfill('0') << static_cast<uint32_t>(payload.at(i));
                    gyro_z_str += ss.str();
                }

                auto parse_asix_data = [this](std::string data, float scale) -> float
                {
                    int v_sign = data.at(0) == '0' ? 1 : -1;
                    data.erase(0, 1);
                    return float(std::stoi(data) * v_sign) / scale;
                };

                Vector9d<float> asix_9_data;
                asix_9_data.axis1 = VectorXd<float>{.axis{parse_asix_data(roll_str, 100.0f), parse_asix_data(pitch_str, 100.0f), parse_asix_data(yaw_str, 100.0f)}};
                asix_9_data.axis2 = VectorXd<float>{.axis{parse_asix_data(acc_x_str, 1000.0f), parse_asix_data(acc_y_str, 1000.0f), parse_asix_data(acc_z_str, 1000.0f)}};
                asix_9_data.axis3 = VectorXd<float>{.axis{parse_asix_data(gyro_x_str, 100.0f), parse_asix_data(gyro_y_str, 100.0f), parse_asix_data(gyro_z_str, 100.0f)}};

                param_->on_asix_data_arrival(DateTime::currentDateTime().toNSecsSinceEpoch(), std::move(asix_9_data));
                break;
            }
            case RSP_8C_CMD:
            {
                sros_info << "RSP_8C_CMD" << sros_endl;
                break;
            }
            case RSP_8B_CMD:
            {
                sros_info << "RSP_8B_CMD" << sros_endl;
                break;
            }
            case RSP_28_CMD:
            {
                sros_info << "RSP_28_CMD" << sros_endl;
                break;
            }
            default:
            {
                std::string reason = util::format("unkown cmd: {}", (int)cmd); 
                on_error(-1, std::move(reason));
                break;
            }
            }
        };

        uart_ = std::make_shared<hal::Uart>(uart_param_);
    }
}